//
//  IMTCANMessageCoder.m
//  ROCOMO
//
//  Created by Kyle Sun on 5/16/16.
//  Copyright © 2016 ROCOMO. All rights reserved.
//

#import "IMTCANMessageCoder.h"
#import "IMTCANMessage.h"

@implementation IMTCANMessageCoder

- (NSData *)dataFromMessage:(IMTCANMessage *)message {
    
    IMTCANMessageBody body = message.body;
    Byte *bytes = (Byte *)malloc(kIMTCANMessageStandardFrameLength);
    memcpy(bytes, &body, kIMTCANMessageStandardFrameLength);
    int offset = 0;
    Byte checkSum = 0x00;
    
    u_int64_t length = kIMTCANMessageStandardFrameLength + message.extra.length;
    
    // 添加消息头
    Byte *finalBytes = (Byte *) malloc(length * 2 + 2 + 2 + 1); //最多就是每个数据都加A5再加上头尾长度和校验和
    finalBytes[offset++] = CANFrameHead;
    finalBytes[offset++] = CANFrameHead;
    
    // 添加标准消息体
    for (int i = 0; i < kIMTCANMessageStandardFrameLength; i++) {
        if (bytes[i] == CANFrameHead || bytes[i] == CANFrameControl || bytes[i] == CANFrameTail) {
            finalBytes[offset++] = CANFrameControl;
        }
        finalBytes[offset++] = bytes[i];
        checkSum += bytes[i];
    }
    free(bytes);
    
    // 添加扩展数据
    if (message.extra) {
        NSUInteger extraLength = message.extra.length;
        Byte *extraBytes = malloc(extraLength);
        [message.extra getBytes:extraBytes length:extraLength];
        for (int i = 0; i < extraLength; i++) {
            if (extraBytes[i] == CANFrameHead || extraBytes[i] == CANFrameControl || extraBytes[i] == CANFrameTail) {
                finalBytes[offset++] = CANFrameControl;
            }
            finalBytes[offset++] = extraBytes[i];
            checkSum += extraBytes[i];
        }
        free(extraBytes);
    }
    
    // 添加校验和
    if (checkSum == CANFrameHead || checkSum == CANFrameControl || checkSum == CANFrameTail) {
        finalBytes[offset++] = CANFrameControl;
    }
    finalBytes[offset++] = checkSum;
    
    // 添加消息尾
    finalBytes[offset++] = CANFrameTail;
    finalBytes[offset++] = CANFrameTail;
    
    NSData *encodedData = [NSData dataWithBytes:finalBytes length:offset];
    free(finalBytes);
    
    return encodedData;
}

- (IMTCANMessage *)messageFromData:(NSData *)data {
    NSData *decodedData = [self decodedDataFromData:data];
    if (!decodedData) {
        return nil;
    }
    
    IMTCANMessageBody body;
    [decodedData getBytes:&body range:NSMakeRange(0, kIMTCANMessageStandardFrameLength)];
    
    // 如果长度为254，说明该消息为扩展格式
    BOOL hasExtra = body.length == 254;
    NSData *extra = nil;
    if (hasExtra) {
        u_int64_t length = body.data & 0x00000000FFFFFFFF;
        body.data = length;
        NSRange range = NSMakeRange(kIMTCANMessageStandardFrameLength, decodedData.length - kIMTCANMessageStandardFrameLength);
        extra = [decodedData subdataWithRange:range];
    }
    IMTCANMessage *message = [[IMTCANMessage alloc] initWithBody:body extra:extra];
    
    return message;
}

- (NSData *)decodedDataFromData:(NSData *)data {
    NSUInteger length = [data length];
    Byte *bytes = (Byte *) malloc(length);
    [data getBytes:bytes length:length];
    NSLog(@"原始数据:%@", [NSData dataWithBytes:bytes length:length]);
    Byte *unpackedBytes = (Byte *) malloc(length);
    int offset = 0;
    Byte checkSum = 0x00;
    
    BOOL lookForNextData = NO;
    BOOL lookForNextTail = NO;
    
    for (int i = 2; i < length; i++) {
        Byte byte = bytes[i];
        
        if (lookForNextData) {
            unpackedBytes[offset++] = byte;
            checkSum += byte;
            lookForNextData = NO;
        } else {
            if (byte == CANFrameControl) {
                lookForNextData = YES;
                continue;
            } else {
                lookForNextData = NO;
            }
            
            if (lookForNextTail) {
                if (byte == CANFrameTail) {
                    lookForNextTail = NO;
                    
                    break;
                }
                lookForNextTail = YES;
            } else {
                if (byte == CANFrameTail) {
                    lookForNextTail = YES;
                } else {
                    lookForNextTail = NO;
                }
            }
            
            unpackedBytes[offset++] = byte;
            checkSum += byte;
        }
    }
    free(bytes);
    
    if (offset > 2) {
        NSLog(@"%x", checkSum);
        checkSum = checkSum - CANFrameTail - unpackedBytes[offset - 2];
        NSLog(@"%x", checkSum);
        if (checkSum == unpackedBytes[offset - 2]) {
            NSData *data = [NSData dataWithBytes:unpackedBytes length:offset - 2];
            NSLog(@"解包成功:%@", data);
            
            free(unpackedBytes);
            return data;
        } else {
            NSData *data = [NSData dataWithBytes:unpackedBytes length:offset - 2];
            NSLog(@"解包失败 check_sum:%d %@ ", checkSum, data);
            free(unpackedBytes);
            return nil;
        }
    } else {
        free(unpackedBytes);
        return nil;
    }
}

@end

